
#include "DaServo.h"

DaServo::DaServo(int servo_index)
{
    if (servo_index >= 0 && servo_index <= 15)
    {
        servoPin = servo_index;
    }
}

DaServo::DaServo(int servo_index, long usrServoDelay)
{
    if (servo_index >= 0 && servo_index <= 15)
    {
        servoPin = servo_index;
    }

    if (usrServoDelay > DEFAULT_SERVO_SPEED)
    {
        servoSpeed = usrServoDelay;
    }
}

void DaServo::init()
{

    this->begin();
    this->setPWMFreq(50); // 50HZ更新频率，相当于20ms的周期

    delay(10);
}

void DaServo::setTarget(int usrTarget)
{
    if (usrTarget > 180)
    {
        usrTarget = 180;
    }
    else if (usrTarget < 0)
    {
        usrTarget = 0;
    }
    servoTarget = usrTarget;
}

int DaServo::getTarget()
{
    return servoTarget;
}

int DaServo::getPosition()
{
    return servoPos;
}

int DaServo::getDistanceToGo()
{
    distance2Go = servoTarget - servoPos;
    return distance2Go;
}

void DaServo::setServoSpeed(int usrServoSpeed)
{
    servoSpeed = usrServoSpeed;
}

int DaServo::getServoSpeed()
{
    return servoSpeed;
}

// 设置num(0-15)号舵机角度为angle(0-180)
uint8_t DaServo::setServoAngle(uint16_t angle)
{
    if (angle < 0)
    {
        angle = 0;
    }
    if (angle > 180)
    {
        angle = 180;
    }
    servoPos = angle;

    uint16_t valueTmp = map(angle, 0, 180, 102, 510);

    return this->setPWM(servoPin, 0, valueTmp);
}

void DaServo::servoUpdate()
{
    distance2Go = servoTarget - servoPos; //获取当前位置与目标位置的距离

    if (distance2Go != 0)
    { //如果当前位置与目标位置间存在距离
      //检查速度延迟参数是否达到设定数值
        if (delayCouter < servoSpeed)
        {                  //如果速度延迟参数尚未达到设定数值
            delayCouter++; //延迟参数自加
        }
        else
        {                    //如果速度延迟参数已经达到设定数值
            delayCouter = 0; //延迟参数复位清零

            if (distance2Go > 0)
            { //如果距离为正
                servoPos++;
                this->setServoAngle(servoPos); //舵机正转一步
            }
            else if (distance2Go < 0)
            { //如果距离为负
                servoPos--;
                this->setServoAngle(servoPos); //舵机反转一步
            }
        }
    }
}
